package org.example.physics_engine.collision.detector;

import javafx.util.Pair;
import org.example.physics_engine.BasicSettings;
import org.example.physics_engine.collision.CollisionFilter;
import org.example.physics_engine.collision.manifold.Penetration;
import org.example.physics_engine.collision.contact.ContactConstraint;
import org.example.physics_engine.collision.SequentialImpulses;
import org.example.physics_engine.collision.manifold.Manifold;
import org.example.physics_engine.collision.manifold.ManifoldSolver;
import org.example.physics_engine.body.PhysicsBody;
import org.example.physics_engine.geometry.Vector2d;
import org.example.physics_engine.geometry.shapes.AbstractShape;

import java.util.ArrayList;
import java.util.List;

//基础的碰撞探测器，包含碰撞检测和碰撞求解
public class BasicCollisionDetector implements CollisionDetector{

    //根据物体列表执行narrowPhase的碰撞检测并获得碰撞流形
    @Override
    public List<Pair<Manifold, Pair<PhysicsBody, PhysicsBody>>> detect(List<PhysicsBody> bodies1, List<PhysicsBody> bodies2, List<CollisionFilter> filters) {
        List<Pair<Manifold, Pair<PhysicsBody, PhysicsBody>>> manifolds = new ArrayList<>();
        ManifoldSolver manifoldSolver = new ManifoldSolver();
        for (PhysicsBody body1 : bodies1) {
            for (PhysicsBody body2 : bodies2) {
                Manifold manifold = this.processDetect(manifoldSolver, body1, body2, filters);
                if (manifold != null) {
                    Pair<PhysicsBody, PhysicsBody> physicsBodyPhysicsBodyPair = new Pair<>(body1, body2);
                    manifolds.add(new Pair<>(manifold, physicsBodyPhysicsBodyPair));
                }
            }
        }
        return manifolds;
    }

    //完整碰撞解析器
    private Manifold processDetect(ManifoldSolver manifoldSolver, PhysicsBody body1, PhysicsBody body2, List<CollisionFilter> filters) {
        AbstractShape shape1 = body1.getShape();
        AbstractShape shape2 = body2.getShape();
        // BroadPhase
        for (CollisionFilter filter : filters) {
            if (!filter.isAllowedBroadPhase(body1, body2)) return null;
        }
        if (!DetectorUtil.AABBDetect(shape1, shape2)) {
            return null;
        }
        // NarrowPhase
        for (CollisionFilter filter : filters) {
            if (!filter.isAllowedNarrowPhase(body1, body2)) return null;
        }
        Penetration penetration = new Penetration();
        DetectorResult detect = DetectorUtil.satDetect(shape1, shape2, null, penetration);
        if (!detect.isHasCollision()) {
            return null;
        }
        // ManifoldSolver
        for (CollisionFilter filter : filters) {
            if (!filter.isAllowedManifold(body1, body2, detect.getApproximateShape(), penetration)) return null;
        }
        Manifold manifold = new Manifold();
        if (!manifoldSolver.getManifold(penetration, shape1, shape2, detect.getApproximateShape(), manifold)) {
            return null;
        }
        return manifold;
    }

    @Override
    public List<ContactConstraint> preLocalSolve(List<Pair<Manifold, Pair<PhysicsBody, PhysicsBody>>> manifolds) {
        List<ContactConstraint> contactConstraints = new ArrayList<>();
        for (Pair<Manifold, Pair<PhysicsBody, PhysicsBody>> manifold : manifolds) {
            ContactConstraint contactConstraint = new ContactConstraint(manifold.getValue());
            contactConstraint.update(manifold.getKey());
            contactConstraints.add(contactConstraint);
        }
        return contactConstraints;
    }

    /*
    * 基础的本地求解器
    * 处理管线较为简单，分为以下几步：
    * 1. 使用重力和引力更新速度和角速度
    * 2. 求解器为碰撞对更新速度和角速度
    * 3. 使用速度和角速度更新位置
    * 4. 求解器反向施加位置冲量防止内嵌
    * */
    @Override
    public void LocalSolve(SequentialImpulses solver, Vector2d gravity, List<ContactConstraint> constraints, List<PhysicsBody> bodies) {
        for (PhysicsBody body : bodies) {
            body.integrateVelocity(gravity);
        }
        //初始化，给碰撞信息
        solver.initialize(constraints);
        //更新碰撞后速度
        for (int i = 0; i < BasicSettings.DEFAULT_SOLVER_ITERATIONS; i++) {
            solver.solveVelocityConstraints(constraints);
        }
        //用速度更新位置（无碰撞时）
        for (PhysicsBody body : bodies) {
            body.integratePosition();
        }
        //位置求解器（有碰撞时）
        for (int i = 0; i < BasicSettings.DEFAULT_SOLVER_ITERATIONS; i++) {
            solver.solvePositionConstraints(constraints);
        }
    }
}
